#include "mcp2515.h"

void mcp2515_init(void)
{
	// Set CNF1 with TQ = 1 and BRP = 1
	mcp2515_write_register(MCP2515_CNF1, 0x01);

	// Set CNF2 with BLT mode, PS1 = 3 and PRSEG = 1
	mcp2515_write_register(MCP2515_CNF2, (1<<7) | (2<<3) | 0);

	// Set CNF3 with PS2 = 3
	mcp2515_write_register(MCP2515_CNF3, 0x02);

	// Set MCP2515 to normal operation mode
	mcp2515_bit_modify(MCP2515_CANCTRL, MCP2515_REQOP_MASK, MCP2515_NORMAL_MODE);

}

void mcp2515_reset(void)
{
	spi_select_can();
	spi_write(MCP2515_RESET);
	spi_select_lcd();
}

uint8_t mcp2515_read_status(void)
{
	spi_select_can();
	spi_write(MCP2515_READ_STATUS);
	uint8_t status = spi_read();
	spi_select_lcd();

	return status;
}

uint8_t mcp2515_read_rx_status(void)
{
	spi_select_can();
	spi_write(MCP2515_READ_RX_STATUS);
	uint8_t status = spi_read();
	spi_select_lcd();

	return status;
}

uint8_t mcp2515_read_register(uint8_t address)
{
	spi_select_can();
	spi_write(MCP2515_READ_REGISTER);
	spi_write(address);
	uint8_t value = spi_read();
	spi_select_lcd();

	return value;
}

void mcp2515_read_receive_buffer(uint8_t buffer, uint8_t* identifier, uint8_t* data, uint8_t* len)
{
	uint8_t buffer_code = 0;
	if(buffer == 0)
		buffer_code = 0x00;
	if(buffer == 1)
		buffer_code = 0x06;

	spi_select_can();
	spi_write(MCP2515_READ_RX_BUFFER | buffer_code);
	
	// Read standard id registers
	uint8_t id_high = spi_read();
	uint8_t id_low = spi_read();

	*identifier = (id_high << 3) | (id_low >> 5);

	// Read extended id registers
	/*uint8_t ext_id_high = */spi_read();
	/*uint8_t ext_id_low = */spi_read();

	// Read length
	uint8_t length = (spi_read() & 0x0F);
	*len = length;

	// Read data registers
	uint8_t i = 0;
	for(i = 0; i < length; i++)
		data[i] = spi_read();
	
	spi_select_lcd();
	
	return;
}

void mcp2515_bit_modify(uint8_t address, uint8_t mask, uint8_t data)
{
	spi_select_can();
	spi_write(MCP2515_BIT_MODIFY);
	spi_write(address);
	spi_write(mask);
	spi_write(data);
	spi_select_lcd();
}

void mcp2515_write_register(uint8_t address, uint8_t data)
{
	spi_select_can();
	spi_write(MCP2515_WRITE_REGISTER);
	spi_write(address);
	spi_write(data);
	spi_select_lcd();
}

void mcp2515_request_to_send(uint8_t buffers)
{
	// Command bits are 1 0 0 0 0 T2 T1 T0
	spi_select_can();
	spi_write(MCP2515_RTS | buffers);
	spi_select_lcd();
}

void mcp2515_load_transmit_buffer(uint8_t buffer, uint8_t identifier, uint8_t* data, uint8_t len)
{
	uint8_t i = 0;
	
	uint8_t id_low = (0x07 & identifier) << 5;
	uint8_t id_high = ((0xF8 & identifier) >> 3);

	spi_select_can();

	spi_write(MCP2515_LOAD_TX_BUFFER | buffer);

	// Write standard identifier registers
	spi_write(id_high);
	spi_write(id_low);

	// Write extended identifier registers
	spi_write(0x00);
	spi_write(0x00);

	// Write data length
	spi_write(len);

	for(i = 0; i < len; i++)
		spi_write(data[i]);

	spi_select_lcd();
}

void can_transmit_message(uint8_t identifier, uint8_t* data, uint8_t len)
{
	// Find empty transmit buffer

	mcp2515_load_transmit_buffer(0, identifier, data, len);

	mcp2515_request_to_send(0x01);
}

void can_read_message(uint8_t* id, uint8_t* data, uint8_t* len)
{
	// Read status from mcp2515
	uint8_t status = mcp2515_read_rx_status();

	// Check if received flag is set for buffer
	if(status & MCP2515_RXSTATUS_RXB0)
		mcp2515_read_receive_buffer(0, id, data, len);
	else if(status & MCP2515_RXSTATUS_RXB1)
		mcp2515_read_receive_buffer(1, id, data, len);
	
}

// on_off = 0 = headlights off
// on_off = 1 = headlights on
void can_message_headlights(uint8_t on_off)
{
	uint8_t data[1] = { on_off };
	can_transmit_message(CAN_MSG_HEADLIGHTS_ID, data, 1);
}

// on_off = 0 = both off
// on_off = 1 = left on
// on_off = 2 = right on
// on_off = 3 = both on
void can_message_directionlights(uint8_t on_off)
{
	uint8_t data[1] = { on_off };
	can_transmit_message(CAN_MSG_DIRLIGHTS_ID, data, 1);
}

// servo = 1 selects servo1, servo = 2 selects servo2
// position selects position of servo motor. Must be between 600 and 2400.
void can_message_servo_position(uint8_t servo, uint16_t position)
{
	uint8_t id;
	if(position < 600 || position > 2400)
		return;
	
	if(servo == 1)
		id = CAN_MSG_SERVO1_POS_ID;
	if(servo == 2)
		id = CAN_MSG_SERVO2_POS_ID;
	
	uint8_t pos_high= (0xFF00 & position) >> 8;
	uint8_t pos_low = 0x00FF & position;
	uint8_t data[2] = { pos_low, pos_high };
	can_transmit_message(id, data, 2);
}

// Reads temperature value from CAN-device
int16_t can_message_get_temperature(void)
{
	uint8_t null_data[0];
	can_transmit_message(CAN_MSG_GET_TEMP_ID, null_data, 0);

	uint8_t len[1];
	uint8_t id[1];
	uint8_t data[8];

	can_read_message(id, data, len);

	if(id[0] == CAN_MSG_GET_TEMP_RESPONSE)
	{
		if(len[0] == 2)
			return ((data[1] << 8) | data[0]);
	}
	return 0xC000;
}
